#include "tools/klib.h"
#include "tools/log.h"
#include "comm/cpu_instr.h"

void kernel_strcpy(char *dest, const char *src)
{
    if (!dest || !src)
        return;

    while (*dest && *src)
        *dest++ = *src++;
    *dest = '\0';
}

void kernel_strncpy(char *dest, const char *src, int size)
{
    if (!dest || !src || !size)
        return;

    char *d = dest;
    const char *s = src;

    while (size-- && *s)
        *d++ = *s++;

    if (size == 0)
        *(d - 1) = '\0';
    else if (*s == '\0')
        *d = '\0';
}

int kernel_strncmp(const char *s1, const char *s2, int size)
{
    if (!s1 || !s2)
        return 0;

    while (*s1 && *s2 && *s1++ == *s2++ && size-- > 0);

    return !(*s1 == '\0' || *s2 == '\0' || *s1 == *s2);
}

int kernel_strlen(const char *str)
{
    int len = 0;
    const char *p = str;
    while (p && *p++)
        len++;
    return len;
}

void kernel_memcpy(void *dest, void *src, int size)
{
    uint8_t *d = dest, *s = src;
    while (d && s && size--)
        *d++ = *s++;
}

void kernel_memset(void *dest, uint8_t v, int size)
{
    uint8_t *d = dest;
    while (d && size--)
        *d++ = v;
}

int kernel_memcmp(void *d1, void *d2, int size)
{
    if (!d1 || !d2 || !size)
        return 1;

    uint8_t *pd1 =  (uint8_t *)d1;
    uint8_t *pd2 =  (uint8_t *)d2;
    while (size--)
        if (*pd1++ != *pd2++)
            return 1;

    return 0;
}

void kernel_itoa(char *buf, int num, int base)
{
    if (base != 2 && base != 8 && base != 10 && base != 16) {
        *buf = '\0';
        return;
    }

    const char *dig2ch = "0123456789ABCDEF";
    char *p = buf;
    int num_dup = num < 0 ? -num : num;
    if (num < 0)
        *p++ = '-';

    do {
        *p++ = dig2ch[num_dup % base];
    } while(num_dup /= base);
    *p-- = '\0';

    // 反转字符串
    char *pl = buf;
    if (num < 0)
        pl++;
    while (pl < p) {
        *pl ^= *p;
        *p ^= *pl;
        *pl ^= *p;
        pl++;
        p--;
    }
}

void kernel_vsprintf(char *strbuf, const char *format, va_list args)
{
    enum {NORMAL, READ_FMT} state = NORMAL;
    char *curr = strbuf;
    char ch;
    while((ch = *format++)) {
        switch (state) {
            case NORMAL:
                if (ch == '%')
                    state = READ_FMT;
                else
                    *curr++ = ch;
                break;
            case READ_FMT:
                if (ch == 's') {
                    const char *s = va_arg(args, char *);
                    while (s && *s)
                        *curr++ = *s++;
                } else if (ch == 'd') {
                    int decimal = va_arg(args, int);
                    kernel_itoa(curr, decimal, 10);
                    curr += kernel_strlen(curr);
                } else if (ch == 'x') {
                    int hex = va_arg(args, int);
                    kernel_itoa(curr, hex, 16);
                    curr += kernel_strlen(curr);
                } else if (ch == 'c') {
                    *curr++ = va_arg(args, int);
                }
                state = NORMAL;
                break;
        }
    }
}

void kernel_sprintf(char *strbuf, const char *format, ...)
{
    va_list args;
    va_start(args, format);
    kernel_vsprintf(strbuf, format, args);
    va_end(args);
}

void pannic(const char *file, int line, const char *func, const char *cond)
{
    log_printf("assert failed! %s", cond);
    log_printf("file: %s line: %d func: %s", file, line, func);
    while(1) {
        hlt();
    }
}